#include "optimizer.h"

Optimizer::Optimizer(std::shared_ptr<ImuPreIntegration> imu_preintegration, std::shared_ptr<WheelPreIntegration> wheel_preintegration) {
    imu_factor_ = std::make_shared<ImuFactor>(imu_preintegration);
    wheel_factor_ = std::make_shared<WheelFactor>(wheel_preintegration);;
}
// qi, pi, vi, bg, ba, qj, pj, vj
void Optimizer::OptimizeCeres(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose) {
    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    loss_function = new ceres::CauchyLoss(1.0);

    ceres::LocalParameterization *pose_i = new Pose2d();

    // double state_i[8];
    // state_i[0] = last_state.R.log();
    // state_i[1] = last_state.v[0];
    // state_i[2] = last_state.v[1];
    // state_i[3] = last_state.p[0];
    // state_i[4] = last_state.p[1];
    // state_i[5] = last_state.bg[0];
    // state_i[6] = last_state.bg[1];
    // state_i[7] = last_state.ba[0];
    // state_i[8] = last_state.ba[1];
    // problem.AddParameterBlock(state_i, 8, pose_i);

    // double state_j[5];

    // problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
